import lejos.nxt.*;
import lejos.robotics.navigation.*;

public class Wallfollow{

	float desiredDistance = 40f;
//	private float diff;
	private int side;
//	private float delta;
	private Pilot p;
	private int sleepTimer = 100;
	Thread t;
	Thread wft;
//	Thread d;
	private Master master;
	private Driver goldieDriver;
	private boolean run =true;
	final float moveSpeed;


	public Wallfollow(Master master, Driver goldieDriver, boolean left, float moveSpeed){
		this.p = p;	
		this.master = master;
		this.goldieDriver = goldieDriver;
		this.moveSpeed = moveSpeed;
		wft = new Thread(new WallFollowThread());
		this.setSideLeft(left);
		wft.start();
//		d.start();
	}

	class WallFollowThread implements Runnable {
		public void run(){
			float diff;
			float delta;
			float firstDistance = 0;
			float lastDiff =0;
			float previousCorrect = 0;
			float alfa = 1f;
			while (true){

				try{
					Thread.sleep(50);
				}catch(InterruptedException e){
				}
				while(run){
					float secondDistance = master.getDistance();	
					//factor voor afgeleide afstand in de tijd
					diff = 100/sleepTimer*(secondDistance - firstDistance)*16f/moveSpeed*30f;//12
					//factor voor afstand tot muur in de tijd
					delta = (secondDistance - desiredDistance)*1.33f;
					firstDistance = secondDistance;
					if(Math.abs(diff)>300){
						diff = 0;
					}
					else{
						lastDiff= diff;
					}

					if(Math.abs(delta)>100){
						delta = 40*Math.signum(delta);

					}
					else{
						lastDiff= diff;
					}

					float correct = (diff+delta)*side;
					if (run)
						goldieDriver.addCommand(new Command(correct*alfa+previousCorrect*(1-alfa), 45f, false, CommandEnum.STEER));
					previousCorrect = correct;
					try{
						Thread.sleep(sleepTimer);
					}catch(InterruptedException e){
					}
				}
			}
		}
	


//	class Diff implements Runnable{
//		public void run(){
//			float first = 0;
//			float lastDiff =0;
//			while(true){
//				try{
//					Thread.sleep(sleepTimer);
//				}catch(InterruptedException e){
//				}
//				float second = mast.getDistance();	
//				diff = 100/sleepTimer*(second - first)*12f;
//				delta = (second - distance)*1f;
//				first = second;
//				//grote diff niet doorgeven
//				float filteredDiff = diff;
//				if(Math.abs(diff)>300){
//					diff = 0;
//
//				}
//				else{
//					lastDiff= diff;
//				}
//
//				if(Math.abs(delta)>100){
//					delta = 40*Math.signum(delta);
//
//				}
//				else{
//					lastDiff= diff;
//				}
//
//
//			}
//		}
	}


//	public float getDifference(){
//		return this.diff;
//	}

	/**
	 * Bepaalt de draairichting van de robot
	 * @param 	left
	 * 			if(true)
	 * 				muur staat links
	 * 			else
	 * 				muur staat rechts
	 */
	public void setSideLeft(boolean left){
		if(left)
			this.side = 1;
		else
			this.side = -1;
	}
	public boolean getSideLeft(){
		return side==1;
	}
	public int getSide(){
		return side;
	}

	public void pause(){
		run = false;
	}

	public void restart(){
		run = true;
	}


	public void setDistance(float distance){
		desiredDistance = distance;
	}

}

